#ifndef _GRIDCUBE_H
#define _GRIDCUBE_H
#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <math.h>
#include <gmm_voxel_map/gmm_voxel_map.h>

using namespace std;

class GridCube
{
public:
	GridCube(){}
	~GridCube(){}

	//Eigen::Vector3d p1, p2, p3, p4, p5, p6, p7, p8;   // the 8 vertex of a cube 
	Eigen::MatrixXd vertex_pos;
	Eigen::Vector3i center_idx; // the center of the cube
	bool valid;    // indicates whether this cube should be deleted
	Eigen::MatrixXi vertex_idx;

	double t; // time allocated to this cube
	std::vector< std::pair<double, double> > box;
	gvm::GMMVoxelMapCircularBuffer* CridCubeMapPtr;
	void GridCubeLinkMap(gvm::GMMVoxelMapCircularBuffer* gvmMap)
	{
		CridCubeMapPtr=gvmMap;
	}


	// create a cube using 8 vertex and the center point

	GridCube(Eigen::Vector3i node_index)
	{
		vertex_idx = Eigen::MatrixXi::Zero(8, 3);
		for(int i=0;i<8;i++)
		{
			vertex_idx.row(i)=node_index;
		}
		vertex_pos= Eigen::MatrixXd::Zero(8, 3);
		center_idx=node_index;
	}

	void setindex(Eigen::Vector3i node_index)
	{
		vertex_idx = Eigen::MatrixXi::Zero(8, 3);
		for(int i=0;i<8;i++)
		{
			vertex_idx.row(i)=node_index;
		}
		vertex_pos= Eigen::MatrixXd::Zero(8, 3);
		center_idx=node_index;		
	}

	bool InflateCube(Eigen::MatrixXi last_InflateCube, double InflateCubeSafetyDistance)
	{

		// Y- now is the left side : (p1 -- p4 -- p8 -- p5) face sweep
		// ############################################################################################################
		bool collide;

		int iter = 0;
		int _max_inflate_iter=100;
		int _step_length=2;
		int id_y,id_x,id_z;
		int _max_y_id=30;
		int _max_x_id=30;
		int _max_z_id=10;
		Eigen::MatrixXi vertex_idx_lst = vertex_idx;
		if(VertexIsContained(last_InflateCube))return false;
		Eigen::MatrixXi initial_vertex=center_idx;

		bool collide_yminus=false;
		bool collide_yplus=false;
		bool collide_xminus=false;
		bool collide_xplus=false;
		bool collide_zminus=false;
		bool collide_zplus=false;

		while(iter < _max_inflate_iter)
		{   
			iter++;
		
			collide  = false; 
			int y_lo = std::max(initial_vertex(1)-_max_y_id, vertex_idx(0, 1) - _step_length);
			int y_up = std::min(initial_vertex(1)+_max_y_id, vertex_idx(1, 1) + _step_length);

			if(!collide_yminus&&vertex_idx(0, 1)!=initial_vertex(1)-_max_y_id)
			{
				for(id_y = vertex_idx(0, 1); id_y >= y_lo; id_y-- )
				{   
					if( collide == true) 
						break;
					
					for(id_x = vertex_idx(0, 0); id_x >= vertex_idx(3, 0); id_x-- )
					{    
						if( collide == true) 
							break;

						for(id_z = vertex_idx(0, 2); id_z >= vertex_idx(4, 2); id_z-- )
						{
							Eigen::Vector3i idx(id_x,id_y,id_z);
							if(  CridCubeMapPtr->getMapLocalIdxEdt(idx)<InflateCubeSafetyDistance)
							{
								collide = true;
								collide_yminus=true;
								break;
							}
						}
					}
				}

				if(collide)
				{
					vertex_idx(0, 1) = std::min(id_y+2, vertex_idx(0, 1));
					vertex_idx(3, 1) = std::min(id_y+2, vertex_idx(3, 1));
					vertex_idx(7, 1) = std::min(id_y+2, vertex_idx(7, 1));
					vertex_idx(4, 1) = std::min(id_y+2, vertex_idx(4, 1));
				}
				else
					vertex_idx(0, 1) = vertex_idx(3, 1) = vertex_idx(7, 1) = vertex_idx(4, 1) = id_y + 1;
			}



			// Y+ now is the right side : (p2 -- p3 -- p7 -- p6) face
			// ############################################################################################################

			if(!collide_yplus&&vertex_idx(1, 1)!=initial_vertex(1)+_max_y_id)
			{
				collide = false;

				for(id_y = vertex_idx(1, 1); id_y <= y_up; id_y++ )
				{   
					if( collide == true) 
						break;
					
					for(id_x = vertex_idx(1, 0); id_x >= vertex_idx(2, 0); id_x-- )
					{
						if( collide == true) 
							break;

						for(id_z = vertex_idx(1, 2); id_z >= vertex_idx(5, 2); id_z-- )
						{
							Eigen::Vector3i idx(id_x,id_y,id_z);
							if(CridCubeMapPtr->getMapLocalIdxEdt(idx)<InflateCubeSafetyDistance)
							{
								collide = true;
								collide_yplus=true;
								break;
							}
						}
					}
				}

				if(collide)
				{
					vertex_idx(1, 1) = std::max(id_y-2, vertex_idx(1, 1));
					vertex_idx(2, 1) = std::max(id_y-2, vertex_idx(2, 1));
					vertex_idx(6, 1) = std::max(id_y-2, vertex_idx(6, 1));
					vertex_idx(5, 1) = std::max(id_y-2, vertex_idx(5, 1));
				}
				else
					vertex_idx(1, 1) = vertex_idx(2, 1) = vertex_idx(6, 1) = vertex_idx(5, 1) = id_y - 1;
			}
		

			// X + now is the front side : (p1 -- p2 -- p6 -- p5) face
			// ############################################################################################################
			int x_lo = std::max(initial_vertex(0)-_max_x_id, vertex_idx(3, 0) - _step_length);
			int x_up = std::min(initial_vertex(0)+_max_x_id, vertex_idx(0, 0) + _step_length);

			if(!collide_xplus&&vertex_idx(0, 0)!=initial_vertex(0)+_max_x_id)
			{
				collide = false;
				for(id_x = vertex_idx(0, 0); id_x <= x_up; id_x++ )
				{   
					if( collide == true) 
						break;
					
					for(id_y = vertex_idx(0, 1); id_y <= vertex_idx(1, 1); id_y++ )
					{
						if( collide == true) 
							break;

						for(id_z = vertex_idx(0, 2); id_z >= vertex_idx(4, 2); id_z-- )
						{
							Eigen::Vector3i idx(id_x,id_y,id_z);
							if(CridCubeMapPtr->getMapLocalIdxEdt(idx)<InflateCubeSafetyDistance)
							{
								collide = true;
								collide_xplus=true;
								break;
							}
						}
					
					}
				}

				if(collide)
				{
					vertex_idx(0, 0) = max(id_x-2, vertex_idx(0, 0)); 
					vertex_idx(1, 0) = max(id_x-2, vertex_idx(1, 0)); 
					vertex_idx(5, 0) = max(id_x-2, vertex_idx(5, 0)); 
					vertex_idx(4, 0) = max(id_x-2, vertex_idx(4, 0)); 
				}
				else
					vertex_idx(0, 0) = vertex_idx(1, 0) = vertex_idx(5, 0) = vertex_idx(4, 0) = id_x - 1; 
			}
	

			// X- now is the back side : (p4 -- p3 -- p7 -- p8) face
			// ############################################################################################################

			if(!collide_xminus&&vertex_idx(3, 0)!=initial_vertex(0)-_max_x_id)
			{
				collide = false;
				for(id_x = vertex_idx(3, 0); id_x >= x_lo; id_x-- )
				{   
					if( collide == true) 
						break;
					
					for(id_y = vertex_idx(3, 1); id_y <= vertex_idx(2, 1); id_y++ )
					{
						if( collide == true) 
							break;

						for(id_z = vertex_idx(3, 2); id_z >= vertex_idx(7, 2); id_z-- )
						{
							Eigen::Vector3i idx(id_x,id_y,id_z);
							if(CridCubeMapPtr->getMapLocalIdxEdt(idx)<InflateCubeSafetyDistance)
							{
								collide = true;
								collide_xminus=true;
								break;
							}
						}
					}
				}

				if(collide)
				{
					vertex_idx(3, 0) = min(id_x+2, vertex_idx(3, 0)); 
					vertex_idx(2, 0) = min(id_x+2, vertex_idx(2, 0)); 
					vertex_idx(6, 0) = min(id_x+2, vertex_idx(6, 0)); 
					vertex_idx(7, 0) = min(id_x+2, vertex_idx(7, 0)); 
				}
				else
					vertex_idx(3, 0) = vertex_idx(2, 0) = vertex_idx(6, 0) = vertex_idx(7, 0) = id_x + 1;
			}


		
			// Z+ now is the above side : (p1 -- p2 -- p3 -- p4) face
			// ############################################################################################################
			collide = false;
			int z_lo = max(initial_vertex(2)-_max_z_id, vertex_idx(4, 2) - _step_length);
			int z_up = min(initial_vertex(2)+_max_z_id, vertex_idx(0, 2) + _step_length);

			if(!collide_zplus&& vertex_idx(0, 2)!=initial_vertex(2)+_max_z_id)
			{
				for(id_z = vertex_idx(0, 2); id_z <= z_up; id_z++ )
				{   
					if( collide == true) 
						break;
					
					for(id_y = vertex_idx(0, 1); id_y <= vertex_idx(1, 1); id_y++ )
					{
						if( collide == true) 
							break;

						for(id_x = vertex_idx(0, 0); id_x >= vertex_idx(3, 0); id_x-- )
						{
							Eigen::Vector3i idx(id_x,id_y,id_z);
							if(CridCubeMapPtr->getMapLocalIdxEdt(idx)<InflateCubeSafetyDistance)
							{
								collide = true;
								collide_zplus=true;
								break;
							}
						}
					}
				}

				if(collide)
				{
					vertex_idx(0, 2) = max(id_z-2, vertex_idx(0, 2));
					vertex_idx(1, 2) = max(id_z-2, vertex_idx(1, 2));
					vertex_idx(2, 2) = max(id_z-2, vertex_idx(2, 2));
					vertex_idx(3, 2) = max(id_z-2, vertex_idx(3, 2));
				}
				else
					vertex_idx(0, 2) = vertex_idx(1, 2) = vertex_idx(2, 2) = vertex_idx(3, 2) = id_z - 1;
			}


			// now is the below side : (p5 -- p6 -- p7 -- p8) face
			// ############################################################################################################

			if(!collide_zminus&&vertex_idx(4, 2)!=initial_vertex(2)-_max_z_id)
			{
				collide = false;
				for(id_z = vertex_idx(4, 2); id_z >= z_lo; id_z-- )
				{   
					if( collide == true) 
						break;
					
					for(id_y = vertex_idx(4, 1); id_y <= vertex_idx(5, 1); id_y++ )
					{
						if( collide == true) 
							break;

						for(id_x = vertex_idx(4, 0); id_x >= vertex_idx(7, 0); id_x-- )
						{
							Eigen::Vector3i idx(id_x,id_y,id_z);
							if(CridCubeMapPtr->getMapLocalIdxEdt(idx)<InflateCubeSafetyDistance)
							{
								collide = true;
								collide_zminus=true;
								break;
							}
						}
					}
				}

				if(collide)
				{
					vertex_idx(4, 2) = min(id_z+2, vertex_idx(4, 2));
					vertex_idx(5, 2) = min(id_z+2, vertex_idx(5, 2));
					vertex_idx(6, 2) = min(id_z+2, vertex_idx(6, 2));
					vertex_idx(7, 2) = min(id_z+2, vertex_idx(7, 2));
				}
				else
					vertex_idx(4, 2) = vertex_idx(5, 2) = vertex_idx(6, 2) = vertex_idx(7, 2) = id_z + 1;
			}

			if(vertex_idx_lst == vertex_idx)break;
			vertex_idx_lst = vertex_idx;

			if(VertexIsContained(last_InflateCube))return false;
		}
		// ROS_INFO("iter: %d",iter);
		return true;

	}
	bool VertexIsContained(Eigen::MatrixXi last_InflateCube)
	{
		if( vertex_idx(0, 0) <= last_InflateCube(0, 0) && vertex_idx(0, 1) >= last_InflateCube(0, 1) && vertex_idx(0, 2) <= last_InflateCube(0, 2) &&
			vertex_idx(6, 0) >= last_InflateCube(6, 0) && vertex_idx(6, 1) <= last_InflateCube(6, 1) && vertex_idx(6, 2) >= last_InflateCube(6, 2)  )
			return true;
		else
			return false; 
	}
	void UpdateVertexPos()
	{
		for(int i=0;i<8;i++)
		{
			Eigen::Vector3d tmp=CridCubeMapPtr->getMapGlobalPosfromLocalIdx(vertex_idx.row(i));
			vertex_pos.row(i)=tmp;
		}
	}


};



#endif